package model;
/**
 * @author wujiazhan
 * @className Controller
 * @description 起摆稳摆控制器
 * @date 2021-06-05 15:51
 */
@SuppressWarnings("all")
public class PIDController {

    private double kp;
    private double ki;
    private double kd;
    private double samplingTime;//采样时间
    private double output_max;//输出最大值
    private double output_min;//输出最小值

    private Differential differential;//微分器
    private Integrator integrator;//积分器

    public PIDController(double kp, double ki, double kd, double samplingTime, double output_max, double output_min){
        this.kp = kp;
        this.ki = ki;
        this.kd = kd;
        this.samplingTime = samplingTime;
        this.output_max = output_max;
        this.output_min = output_min;
        differential = new Differential(samplingTime);
        integrator = new Integrator(samplingTime);
    }

    //PID控制器

    public double forward(double input_cur){
        double kp_res = kp * input_cur;
        double ki_res = ki * integrator.getIntegralSum(input_cur);
        double kd_res = kd * differential.getDiff(input_cur);

        double res = kp_res + ki_res + kd_res;

        //输出限幅
        if(res > output_max) {
            res = output_max;
        }
        if(res < output_min) {
            res = output_min;
        }
        return res;
    }
    //更新微分器中上一拍的值
    public void setDiffPre_val(double pre_val){
        this.differential.setPre_val(pre_val);
    }
    //更新积分器中上一拍积分的总和
    public void setIntegPre_val(double pre_val){
        this.integrator.setPre_val(pre_val);
    }
}
